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NOTATION 


The following is a summary of some of the notation and conventions used throughout 
this report: 


1. The coordinate frames of the robot are labeled 1-3 (platform) and 4-9 
(PUMA). Frame 0 is the inertial frame, and frame E is the end-effector frame. 

2. k Pi j € B 3 is the vector describing the position of frame j with respect to frame 
i, expressed in the coordinates of frame k. Note that = — k pj ti , 

3. )R € ft 3 * 3 is the rotation matrix describing the orientation of frame j with 
respect to frame i. 


4. )T € 3f 4x4 is the homogeneous transformation describing the position and 
orientation of frame j with respect to frame i: 

}R Pi j ' 

o 1 

5- k Pi,j € 3^ x3 is the cross product matrix associated with the vector k pij, 
expressed in the coordinates of frame k: 




0 ~ k Pij( z ) k Pi.j(y) 

k p>,A z ) o - k pij(x ) 


[ - k p>\j( y) k Pi.A x ) o J 

where k pij(x ), k p l . J (y), and k pi,j(z) are the components of k p i% j . By “cross 
product matrix”, it is meant that, for all w € K 3 , 


k — 




IV = 




x w 


ix 


Note that l k R k p iyJ k R == 


6. k duij € 3? 6 is the differential displacement of frame j with respect to frame 
i , expressed in the coordinates of frame k. The first three components of this 
vector are the differential translation and the last three are the differential 
rotation: 




k d P ij 


7. k Ji,j € 3? 6xn is the Jacobian relating differential joint displacements to the 
differential displacement of frame j with respect to frame z, expressed in the 
coordinates of frame k. 


8. k $j,i € 3£ 6x6 is the transformation that maps k Jij to k Jij: 



0 I 


9. ”R € 3? 6x6 is the transformation that maps k J{j to m Jij: 


m 

k 



?R 0 
0 


10. Trigonometric functions may be abbreviated by their first letter; for example, 
Si = sin ( qi ) and Cij = cos (< 7 ; + qj). 
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ABSTRACT 


This report investigates the disturbance rejection control problem for a 6-DOF 
PUMA manipulator mounted on a 3-DOF platform. A control algorithm is designed 
to track the desired position and attitude of the end-effector in inertial space, sub- 
ject to unknown disturbances in the platform axes. Conditions for the stability of 
the closed-loop system are derived. The performance of the controller is compared 
for step, sinusoidal, and random disturbances in the platform rotational axis and in 
the neighborhood of kinematic singularities. 


xn 




CHAPTER 1 
INTRODUCTION 


1.1 Motivation 

One of the main research objectives at the Center for Intelligent Robotic Sys- 
tems for Space Exploration (CIRSSE) is to demonstrate the feasibility of using 
robotic manipulators for on-orbit tasks. In particular, robotic manipulators have 
been proposed as a means of reducing the amount of extra vehicular activity (EVA) 
time required for space station assembly and maintenance. The proposed scenario 
involves a robotic manipulator attached to some mobile platform, such as a space- 
craft, satellite, or the space station itself. 

Although certain on-orbit tasks will require only joint-space control, others 
will require motion with respect to an inertial or Local Vertical Local Horizontal 
(LVLH) reference frame [1]. In the latter case, disturbances in the platform position 
and attitude may prevent the manipulator from successfully completing the task. 
One possibility is to make course corrections using reaction wheels or jets; however, 
the disturbances may exceed the saturation limits of the reaction mechanism [2]. 
Additionally, this approach could lead to excessive attitude control fuel consump- 
tion, limiting the useful on-orbit life of the system [3]. This report explores a second 
possibility, namely, using the manipulator to compensate for platform disturbances. 

1.2 Past Research 

The problem of controlling a robotic manipulator on a mobile platform has 
received considerable attention in the past few years. Joshi and Desrochers [4. 5] 
designed a nonlinear feedback control law to carry out tasks (with respect to the 
robot base frame) in the presence of roll, pitch and yaw disturbances in the platform 
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axes. Dubowsky, Vance, and Torres [2] proposed a time-optimal planning algorithm 
for a robotic manipulator mounted on a spacecraft, subject to saturation limits 
in the attitude control reaction jets. Papadopoulos and Dubowsky [3] developed 
a general framework for analyzing the control of free-floating space manipulator 
systems. Most recently, Torres and Dubowsky [6] have presented a technique called 
the enhanced disturbance map to find manipulator trajectories that reduce the effect 
of disturbances in the spacecraft position and attitude. 

One common assumption in the literature is that the disturbance signal is 
exactly known. If this is the case, then the end-effector location can be calculated 
without relying on direct end-point sensing. However, this assumption is invalid if 
there is a significant delay in the platform position and attitude measurements, or if 
the kinematics of the platform are not well known, or if the platform is a non-rigid 
structure (such as the proposed Space Station Freedom [7]). In the more likely case 
that only the nominal platform location and upper bound on the disturbance signal 
are known, direct end-point sensing is needed to measure the end-effector location. 

1.3 Report Objective and Organization 

The goal of this report is to investigate the problem of controlling a robotic 
manipulator in the presence of disturbances in the platform axes. Specifically, a 
controller is designed to track the desired position and attitude of the end-effector 
with respect to the inertial reference frame using end-point feedback. The platform 
operating point and the maximum deviation from the operating point are assumed 
to be known. The controller design, analysis, implementation, and performance are 
illustrated for a 6-DOF PUMA manipulator mounted on a 3-DOF platform. 

The remainder of this report is organized as follows: 
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Chapter 2 defines a transformation from task space to joint space, called the ap- 
proximate pseudoinverse Jacobian, which is both singularity-free and compu- 
tationally efficient. 

Chapter 3 examines the disturbance rejection control problem from a kinematic 
perspective and develops a control law for disturbance rejection based on the 
approximate pseudoinverse Jacobian. 

Chapter 4 describes CIRSSE’s robotic testbed and the software implementation 
of the controller on the testbed. 

Chapter 5 presents several sets of experimental results. The performance of the 
controller is compared for various classes of disturbance signals and at the 
singularities of the Jacobian. 

Chapter 6 summarizes this report and discusses some future directions for this 
area of research. 



CHAPTER 2 

THE APPROXIMATE PSEUDOINVERSE JACOBIAN 


In the inertial-space control problem, the desired end-effector trajectory is specified 
in task coordinates (in this case, inertial coordinates), while the actual control takes 
place on the joint level. Hence, some mapping between task and joint space is re- 
quired. For disturbance rejection, the transformation that maps the displacement 
of the end-effector to joint displacements, i.e., the inverse Jacobian, is of particular 
interest. However, this transformation is ill-defined for certain manipulator con- 
figurations. This chapter presents an alternative mapping, called the approximate 
pseudoinverse Jacobian, which is defined for all manipulator configurations. 

There are five sections in this chapter. Section 2.1 discusses the forward and 
inverse Jacobians for the 6-DOF PUMA arm. Section 2.2 reviews the singular 
value decomposition and the pseudoinverse. Section 2.3 presents the definition and 
properties of the approximate pseudoinverse Jacobian. Section 2.4 compares the 
pseudoinverse and approximate pseudoinverse near singularities, as well as the cost 
of computing each solution. Finally, Section 2.5 summarizes the main results from 
this chapter. 

2.1 Background 

The Jacobian matrix (or Jacobian) is a mapping from joint space to task 
(Cartesian) space. It maps differential changes in joint position to differential 
changes in Cartesian position and orientation according to the following relationship: 

du = J{q)dq (2.1) 

where du € is the differential Cartesian displacement vector (linear and angular). 
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q e £ n is the vector of joint positions, dq € 3?" is the vector of differential joint 
displacements, and J € 3? 6xn is the Jacobian matrix. The Jacobian also relates 
joint velocities to Cartesian velocities. 

The inverse mapping, when it exists, is given by 

dq = J~\q)du (2.2) 

In order for J~ l to exist, J must be square (6 x 6) and full rank. The singularities of 
J are those points where the Jacobian loses rank, i.e., rank( J) < 6. The singularities 
of the PUMA Jacobian are discussed further in Section 2.1.4. 

2.1.1 Coordinate Frames 

The kinematic frames of the PUMA and platform are shown in Figure 2.1. The 
coordinate frame assignments follow the Modified Denavit-Hartenberg convention, 
in which coordinate frame i is attached to link i, with the origin on the axis of 
joint i [8]. The kinematic parameters of the PUMA and platform are listed in 
Table 2.1. 


frame 
number, i 

<*1-1 

a,_i 

(m) 

di 

M 

0i 

1 

-90° 

0.32000 

<7 1 

0° 

2 

o 

O 

Ob 

0.00000 

0.54400 

<72 

3 

-90° 

0.00000 

0.00000 

<73 

4 

90° 

0.00000 

0.82800 

<74 

5 

-90° 

0.00000 

0.24300 

<75 

6 

0° 

0.43182 

-0.09391 

<76 

7 

CO 

O 

o 

-0.02031 

0.43300 

<77 

S 

1 

o 

o 

0.00000 

0.00000 

<78 

9 

90° 

0.00000 

0.00000 

<79 


Table 2.1: Kinematic Parameters 




Figure 2.1: Coordinate Frame Assignments 
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2.1.2 Velocity and Coordinate Frame Transformations 

The notation k du l j will be used to denote the differential displacement of 
-frame j with respect to frame z, expressed in the coordinates of frame k. With this 
notation, Equation (2.1) is written as 

k duij = h Jijdq (2.3) 

Frames i and j will be referred to as the reference frame and the velocity frame , 
respectively. Frame k will be referred to as the coordinate frame. 

The velocity frame of the Jacobian can be changed through the transformation 


% = 




k J 


= k $i k J 


(2.4) 


where k pjj is the cross product matrix associated with the position vector k pjj (cf. 
the Notation at the beginning of this report). This transformation is useful for 
finding the differential displacement of the end-effector, k du itE . given the position 
vector k pg iE (obtained from the tool transform) and the Jacobian k J li9 . 

The coordinate frame of the Jacobian can be changed via the transformation 


m 


Jij 


A 


JTR o 

0 ?R 

m ry k j 

k Jij 



(2.5) 


where Jf R is the rotation matrix describing the orientation of frame k with respect 
to frame m. Combining (2.4) and (2.5) results in 


l Ji.l = 


( 2 . 6 ) 
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2.1.3 Forward and Inverse PUMA Jacobians 

Finding the Jacobian and its inverse expressed in any arbitrary coordinate 
frame can be computationally expensive. However, it is possible to take advantage 
of coordinate frame transformations to find the simplest Jacobian matrix [9]. For 
the PUMA arm, the Jacobian matrix is simplest when expressed in frame 6 [10]: 


>^ 3,9 = 


— (ds + d§)C$& d 7 + d 7 0 0 0 

(<^5 + d 6 )S S6 a 6 + QsCq 06 0 0 0 

O5C5 + 06^56 + drSse 0 0 0 0 0 


— S 56 

— C56 

0 


0 

0 

1 


0 0 — S 7 C 7 S B 


0 -1 

1 0 


0 -c 8 

C 7 S 7 S B 


(2.7) 


Note also that this Jacobian matrix is in lower block triangular form. This is due to 
the geometry of spherical wrist arms; i.e., the fact that the origins of the last three 
frames coincide. The following compact notation will be used to denote the matrix 


6 J- 


3 , 9 - 


6 


•^3.9 = 


B 0 
D E 


(2.8) 


where B, D, and E are 3x3 submatrices of the Jacobian. 

The inverse Jacobian, when it exists, can also be written in block matrix form 
(see Kailath [11], p. 656): 


6 7-1 _ 

,v 3.9 


’ B 

0 

-1 - 

_ D 

E _ 



B~ l 0 

-E~ X DB~ X E - 1 


(2.9) 


This expression is only defined when J is full rank, or equivalently, when B and E 
are full rank. The singularities of J are those points where either rank(S) < 3 or 
rank(E) <3. 
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2.1.4 Singularities of the PUMA Jacobian 

The singularities of the PUMA Jacobian can be found by solving for the roots 
of the determinant of J: 


det(J) = det(B) det(£) 

= a s( a 6$6 ~ + a$Cs 6 + d-rS^S^ 

= 0 ( 2 . 10 ) 

When the first factor in (2.10) vanishes, the PUMA is at the Arm Fully Stretched 
singularity. Setting this factor to zero and solving for q& yields 



( 2 . 11 ) 


The Arm Fully Stretched configuration is classified as a workspace boundary singu- 
larity [8]. This singularity occurs whenever the arm switches between the flex and 
the noflex configurations (see [12] for the definitions of the PUMA poses). At the 
Arm Fully Stretched singularity, the end-effector cannot instantaneously move in 
certain linear directions; for example, any differential translation dp which exceeds 
the workspace boundary is physically unachievable. 

The second factor in (2.10) corresponds to the Hand Over Head singularity. 
Setting this factor to zero and solving for q$ yields 


<75 = tan 


-l 


a 5 + + d-jbs 

d~0§ 


+ nx, n = 0, il,±2, . 


( 2 . 12 ) 


The Hand Over Head configuration is a classified as a workspace interior singu- 
larity [8], and corresponds to changing between the right and left configura- 
tions [12]. As in the Arm Fully Stretched singularity, certain instantaneous linear 
directions cannot be achieved at the Hand Over Head configuration. For example, if 
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92 = 93 = <?4 = 0 and Equation (2.12) is satisfied, then instantaneous motion in the 
inertial Y direction is impossible. 

The third factor in (2.10) corresponds to the Wrist singularity. Setting this 
factor to zero and solving for q$ yields 

9s = nir , n = 0,±1,±2,... (2.13) 

The Wrist singularity also occurs in the workspace interior, when the arm switches 
between the flip and noflip configurations [12]. At the Wrist singularity, certain 
instantaneous angular directions cannot be achieved; for example, if the arm is in the 
“home” position (as in Figure 2.1), the end-effector cannot instantaneously rotate 
about the inertial X axis. 

2.2 Pseudoinverse Jacobian 
2.2.1 Motivation 

The usual method of dealing with singularities of the Jacobian is to avoid 

them. For example, Baillieul, Hollerbach, and Brockett [13] proposed using kine- 
matic redundancy 1 to steer around workspace interior singularities. This approach 
is not applicable to the disturbance rejection problem, however, since a sufficiently 
large disturbance could force the manipulator into a singular configuration. There 
are practical problems with singularity avoidance as well. For instance, the manipu- 
lator must avoid not just singular points , but singular regions , since the norm of J~ l 
becomes very large in the neighborhood of a singularity. For disturbance rejection. 

then, it would be desirable to have a mapping from task space to joint space which 
is well-behaved near singularities. This section examines one such candidate, the 
pseudoinverse Jacobian , denoted by J*. 

l A kinematically redundant manipulator has more degrees of freedom than required to reach 
every point in the workspace with arbitrary orientation: hence, n > 6. 
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In robotics literature, the pseudoinverse is often used in the context of path 
planning or control for kinematically redundant manipulators, to overcome the dif- 
ficulty of J being a nonsquare matrix. Roboticists usually define J* as 


(2.14) 


J T (JJ T )~ l m < n 
= < J~ l m = n 
; ' ( J T J)~ l J T m > n 

Clearly, this method of computing J * does not address the issue of singularities 
since it still relies on matrix inversion. A more general approach to computing the 
pseudoinverse, based on the singular value decomposition, is presented below. 


2.2.2 The Singular Value Decomposition 

The singular value decomposition (or “SVD”) is the unique factorization of 
any m x n matrix J into the product of two orthonormal matrices and a matrix 
whose off-diagonal elements are zero and whose diagonal elements are the singular 
values of J. This factorization is expressed below: 


J = UZV T (2.15) 

where U is an m x m orthonormal matrix, V is an n x n orthonormal matrix, and E 
is the m x n matrix of singular values. For notational purposes, it will be assumed 
that m < n, although all of the results are still valid for m > n. 

Since U and V’ are nonsingular, J and E have the same rank. Thus, if 
rank(J) = r. the first r singular values of J will be nonzero and the last m — r 
equal to zero. Furthermore, it can be shown that the singular values are the non- 
negative square roots of the eigenvalues of J J T [14]. Let the singular values be 
ordered as <7] > <Ji > • ■ • > <j m . Then. E is written as 
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0i 


S = 


0r 


E r 0 
0 0 


(2.16) 


The singular values can be used to measure how close J is to being singular. 
One such measure, the condition number, is defined below: 


1 / T \ ^ ^1 

cond(J) = — 

& m 


(2.17) 


A matrix is nearly singular or ill-conditioned when the condition number is very 
large (or infinite). Another measure of singularity, commonly used by roboticists, is 
the measure of manipulability [15]: 


MOM(J) = y/det (IF) 

= v /det(EE 7 ') 

= a iO ~2 • ■ • cr m 

The measure of manipulability behaves like the inverse of the condition number, in 
that MOM(J) — * 0 as cond (J) — ♦ oo. If J is square, it is easy to see that 


MOM(J) = det(J) (2.18) 

2.2.3 Definition of the Pseudoinverse 

Consider for the moment the task of inverting T when m = n. If I is full 
rank, then all of the singular values will be nonzero, and the inverse is simply 
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E- 1 = diag(-, ) (2.19) 

< 7 1 <*1 &m 

In the event that a singular value <T; is zero, E _1 does not exist. The pseudoinverse 
is defined by replacing these l/cr.-’s with zero: 


£ f = diag(— , — 1,0 0) 

C\ <T 2 <J T 


( 2 . 20 ) 


By this definition, a singular value must be exactly zero for E to be singular. How- 
ever, E will be numerically ill-conditioned when one or more of the <7;’s axe very 
small. In practice, it is useful to define a singular value threshold, cr min , below which 
any singular value is considered to be zero. 

For the general case when E is not necessarily square, the pseudoinverse is 
defined as 


X 


E f ± 


i 




0 


S7 1 0 
0 0 


(2.21) 


L 0 J 

The concept of the pseudoinverse can easily be extended to arbitrary matrices. 
Recall that the singular value decomposition factors J into the product U EV r . 
Since the matrices U and V are orthonormal. U~ l = U T and V~ l = V T . Thus, the 
pseudoinverse of J is 


± VE f U T 


( 2 . 22 ) 
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2.2.4 The Moore-Penrose Conditions 

The pseudoinverse can also be defined by four algebraic properties, known as 
the Moore-Penrose conditions: 


JJ'J = J (2.23) 

J*JJ* = (2.24) 

(Jji) T = JJ' (2.25) 

(J + J) T = J*J (2.26) 


The first condition (Equation (2.23)) is also the definition of a generalized inverse. 
That is, any matrix J~ which satisfies the property JJ J = J is a generalized 
inverse of J. Similarly, (2.24) is the definition of a reflexive generalized inverse [15]. 
It is straightforward to verify that is the unique matrix that satisfies all four 
conditions [14]. 

2.2.5 Properties of the Pseudoinverse 

Several important properties of the pseudoinverse are listed below. 

1. If J is square, then J* = J~ x when J is nonsingular. 

2. If J is singular and du £ then there are an infinite number of vectors 

dq that satisfy Equation (2.1). The pseudoinverse selects the least-squares 
solution; that is, dq = J^du is the solution with the smallest 2-norm. 

3. If J is singular and du £ then there are no vectors dq that satisfy (2.1). 

The pseudoinverse constructs a "solution” vector that minimizes the norm of 
the residual; that is, dq = J'du minimizes \\Jdq — du]j 2 . 
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There are many other interesting properties of the pseudoinverse and the sin- 
gular value decomposition that are not directly related to this discussion. The reader 
is referred to [14] or [16] for additional information. 


2.3 Approximate Pseudoinverse Jacobian 
2.3.1 Motivation 

The pseudoinverse has one serious drawback, which is the high cost of com- 
puting the singular value decomposition. The SVD algorithm uses a series of House- 
holder transformations to reduce the input matrix to diagonal form [17]. Since this 
is an 0(N 3 ) operation, finding the SVD for the 6x6 Jacobian matrix can be too 
costly to implement in real-time (see Table 2.2 at the end of this chapter). 

This motivated the search for yet another alternative to the inverse Jaco- 
bian, with the additional constraint that the number of computations be kept to 
a minimum. The alternative presented in this section is called the approximate 
pseudoinverse Jacobian , and is denoted by J *. 


2.3.2 Definition of the Approximate Pseudoinverse 

The basic idea behind the approximate pseudoinverse is to use the partitioned 
form of J (cf. Equation (2.8)) and perform the SVD on the submatrices B and E. 
This reduces the number of computations by a factor of four, since two 3x3 singular 
value decompositions is an C?(2 (jV/ 2) 3 ) operation. 

The definition of the approximate pseudoinverse Jacobian is 


[ -E‘DB r E- J 

where f?, D , and E are defined as in (2.3). It should be noted that if J had a 
block-diagonal instead of a block-triangular structure (i.e., if the linear and angular 
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subspaces of 3? were completely decoupled) then the approximate pseudoinverse 
would be identical to the pseudoinverse. 

2.3.3 Properties of the Approximate Pseudoinverse 

Several properties of the approximate pseudoinverse are stated below. 

1. «/* = J - ? when J is nonsingular. 

2. J * does not satisfy the Moore-Penrose conditions when J is singular. 

3. Properties (2) — (3) of Section 2.2.5 can be extended to the approximate 

pseudoinverse by partitioning 3? 6 into the linear and angular subspaces. Let 
dp, dj> e ft 3 be the linear and angular components of du, respectively, and let 
d<7i,d<7 2 € J? 3 be the components of dq. Then, the approximate pseudoinverse 
solution is » 


1 

ft* 


. . 




dp 

- 

do 


(2.28) 


B x 0 

-E'DB* E x 

If J is singular, the approximate pseudoinverse finds the minimum norm solu- 
tion as if dp and d<p were decoupled, that is, dq = J x du minimizes \\Bdqx - dp\\ 2 
and \\Edq 2 — dp\\ 2 . 


2.4 Comparison 

2.4.1 Behavior Near Singularities 

Pigure 2.2 compares the 2-norm, or the maximum singular value, of (solid 
curve), J x (dashed curve), and J~ l (dotted curve) in the vicinity of the Hand 
Over Head singularity. Figures 2.3 and 2.4 show the behavior near the Arm Fully 
Stretched and Wrist singularities, respectively. 


q4 * 0 deg 
q6 * 0 deg 
q7 * 0 deg 
q8 * 45 deg 
q9 * 0 deg 


Joint 5 Poanon (deg) 


Figure 2.2: 2-Norms of J* (solid curve), J* (dashed curve), and J 
(dotted curve) Near Hand Over Head Singularity 


q4 * 0 deg 
q5 * 0 deg 
q7 * 0 deg 
q8 » 45 deg 
q9 » 0 deg 


Joint 6 Poauon (deg) 


Figure 2.3: 2-Norms of (solid curve), J* (dashed curve), and J 
(dotted curve) Near Arm Fully Stretched Singularity 











Figure 2.4: 2-Norms of J f (solid curve), J x (dashed curve), and J~ l 
(dotted curve) Near Wrist Singularity 

The discontinuities in IML and Ik'lL occur when the smallest nonzero singu- 
lar value, <7 r , falls below the threshold value, cr min . This threshold is an important 
parameter; setting <7 mm to a relatively small value will shrink the width of the 
“well” about the singular point, thus extending the range over which J' = J~ l and 
J* = ■ The side-effect is that the norm will be very large and highly discontinu- 

ous near the singularity. By the same token, setting cr min to a relatively large value 
will reduce the discontinuity in the norm by increasing the width of the singular 
region. A threshold value of cr mtn = 0.1 was used to generate Figures 2.2 - 2.4. 

2.4.2 Bound on Approximation Error 

. The pseudoinverse and approximate pseudoinverse Jacobians are identical onlv 
when J is nonsingular. In order to characterize the difference in behavior at a 
singularity, some measure of the approximation error is needed. 
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Recall from Section 2.2.4 that a generalized inverse J of a matrix J is defined 
by the property 

JJ-J = J 

Since the pseudoinverse satisfies this property, a reasonable way to measure the 
approximation error is to see “how close” J i is to being a true generalized inverse 
using the following norm: 

\jj' j - 4 

An upper bound on the approximation error will now be derived using this norm. 
Consider the matrix 


JJ X J 


Subtracting J yields 


BB X B 0 

DB'B + EE'D(I - B'B) EE'E 

BB'B 0 

D - (I - EE f )D(I - B X B) EE'E 


(2.29) 


JJ X J - J 


BB X B -B 0 

-(/ - EE X )D(I - B X B) EE X E-E _ 

B(I-B'B) 0 

_ (/ - EE')D{I - B X B) {I-EE')E 


I o' 


B 0 


I-B'B 0 

* 

tq 

1 

o 


. DE . 


0 / _ 


When both B and E are singular, the approximation error is bounded as follows: 
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1 JJ X J - J 



’ I 0 


' B o' 


’ I-B'B 0 ' 

1 = 




1 

12 


1 

>♦— 

i 

o 

t 


( 

Q 


0 


< Ph ( 2 -31) 

If B is nonsingular, a less conservative upper bound can be found: 


| JJ l J - J 
! 2 


I 0 

0 I — EE 1 


< M* 


0 0 

0 E 


Likewise, when E is nonsingular the upper bound reduces to 


(2.32) 


1 JJ X J - J 

1 


B 0 


’ I - B'B 0 


= 




I 

12 


1 

O 

o 
1 


i 

O 



< ||*lla (2-33) 


Finally, if both B and E are nonsingular, the approximate pseudoinverse is identical 
to the pseudoinverse: 


(2.34) 

2.4.3 Computation Time 

Table 2.2 compares the computation times of the the inverse, pseudoinverse, 
and approximate pseudoinverse Jacobians for each coordinate frame. As predicted, 
the approximate pseudoinverse is about four times faster to compute than the pseu- 
doinverse. Note that the computation times are largest for frame 0, since the solution 
is first computed in frame 6 and then transformed into the desired frame k using ^R. 


|jj*j - yj| 2 = 0 
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Coordinate Frame 
k 

Computation Time 

* 7-1 
J 3.E 

k 7 T 

J 3.E 

J 3.E 

0 

1.31 ms 

25.31 ms 

6.38 ms 

1 

1.31 ms 

25.31 ms 

6.38 ms 

2 

1.19 ms 

25.31 ms 

6.25 ms 

3 

1.09 ms 

24.98 ms 

6.11 ms 

4 

0.97 ms 

24.65 ms 

5.98 ms 

5 

0.97 ms 

24.65 ms 

5.98 ms 

6 

0.88 ms 

24.98 ms 

5.98 ms 

7 

0.82 ms 

24.98 ms 

5.84 ms 

8 

0.81 ms 

24.98 ms 

5.85 ms 

9 

0.81 ms 

24.65 ms 

5.85 ms 

E 

0.95 ms 

25.31 ms 

6.11 ms 


Table 2.2: Computation Times for k J 3t l, and k J 3 £ 

Hence, transforming the solution into frame Q requires the most computationally ex- 
pensive rotation matrix. 

The inverse, pseudoinverse, and approximate pseudoinverse Jacobian solutions 
were implemented in the C programming language using the GNU 2 gcc Version 2.2.2 
compiler. The data in Table 2.2 was collected by timing the software on a Motorola 
MVME 147SA-2 Single Board Computer. 


2.5 Summary 

A nonsingular mapping from task space to joint space, the approximate pseu- 
doinverse Jacobian, was denned in this chapter. The approximate pseudoinverse was 
compared to the inverse and pseudoinverse in terms of the computational cost and 
the behavior of the norm near kinematic singularities. From this comparison, it can 
be concluded that the approximate pseudoinverse is the clear choice for real-time 
control. 


2 Copvright (C) 1989, 1991 Free Software Foundation. Inc., 675 Mass Ave, Cambridge. MA. 



CHAPTER 3 

A KINEMATIC CONTROL LAW FOR DISTURBANCE REJECTION 

This chapter focuses on the development and analysis of a control law for disturbance 
rejection based on the approximate pseudoinverse Jacobian. The organization of this 
chapter is as follows. Section 3.1 gives an overview of the inertial-space disturbance 
rejection control problem. Section 3.2 proposes a kinematic control law and develops 
an expression for the closed-loop system. Section 3.3 derives an upper bound on the 
control gain for closed-loop stability. Section 3.4 discusses several controller design 
and implementation issues, and Section 3.5 summarizes this chapter. 

3.1 Overview 

3.1.1 Kinematic vs. Dynamic Control 

Any inertial-space controller must take into account both the kinematics and 
the dynamics of the manipulator. The design approach followed in this report is 
to partition the control into tw’o separate loops: a kinematic loop, which outputs 
position setpoints for each joint based on the inertial-space error, and a dynamic 
loop, which outputs torques for each motor based on the joint-space error. 

There are several advantages to decoupling the control in this manner. First, it 
allows the control designer to build and tune each loop independently. The dynamic 
loop, for example, can be tuned by looking only at the joint-space errors, and the 
kinematic loop can be tuned by assuming that the joint-level control is perfect. A 
second advantage is that the two controllers can run in parallel and at different 
sampling rates, provided that the position setpoints are buffered. For example, the 
dynamic loop could be implemented in hardware at a faster sampling rate than the 
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kinematic loop. Finally, a number of dynamic control laws, such as PID, PD-plus- 
gravity, computed-torque, and sliding mode control, have already been developed 
for robot manipulators [18]. The remainder of this chapter will concentrate on the 
kinematic control loop, with the assumption that a dynamic controller is already 
available. 

3.1.2 Problem Formulation 

The control problem that will be addressed in this chapter can be briefly stated 
as follows. Consider a 6-DOF PUMA manipulator mounted on a 3-DOF platform. 
The goal is to maintain the desired position and attitude of the end-effector with 
respect to the inertial reference frame (frame 0), subject to arbitrary disturbances 
in the platform axes. The following information is assumed to be available: 

1. 9 £ 3 ? 6 , the PUMA joint positions 

2. tj q € 3R 3 , the nominal platform joint positions 

3. 6 € 9J 3 , the maximum deviations from the nominal platform joint positions 

4. 0 u 0i £ € 3? 6 , the inertial end-effector location 

Two factors contribute to the motion of the end-effector: the differential dis- 
placement of the PUMA joints, which can be measured, and the differential displace- 
ment of the platform joints, which is unknown. Let 5 denote the disturbance signal 
and let dv be the component of the end-effector motion caused by the differential 
displacement of the platform joints. Then, the differential end-effector displacement 
can be written as 

°du Q , s = °J 3 ' B {Ti 0 + 8,d)dO-dv 

= °R (Ti o + 8) 3 J 3 . E {0)de + dv 


(3.1) 
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Note that coordinate frame transformations have been applied to isolate the depen- 
dence of the PUMA Jacobian on the platform joint positions. 

3.2 Discrete-Time System Analysis 
3.2.1 Discrete-Time Approximation 

A discrete-time model of the system will now be derived by approximating the 
differential quantities in (3.1) with displacements. The underlying assumption here 
is that the sampling period, AT, is sufficiently small (i.e., the sampling rate is much 
higher than the bandwidth of the system). 

Define Au* as A u k = u k - ujt-i, where the subscript k denotes the £th sample 
step. In the limit as AT goes to zero, the displacement An* equals the differential 
du: 


Jfeo Auk = du (3.2) 

Similarly, A 6 k * d& and At^ — * dv as AT — ► 0. Therefore, the discrete-time ap- 
proximation is 

du as A Uk = Uk — Uk-i 
d6 as A Ok = Ok — Qk-i 

dv a: Au fc = v k - (3.3) 

and the discrete version of (3.1) is 

°u k - °u,_, = °R (t) 0 4- S k ) 3 J 3tE (9 k ) A0* 4- At* (3.4) 

where the subscripts denoting the reference and velocity frames of du have been 
dropped to avoid confusion with the time index. 
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3.2.2 Proposed Control Law 

Let °uj be the desired position and orientation of the end-effector along some 
specified trajectory. The control objective is to drive the end-effector to this position 
and orientation: 


°u k -* °Ud as k — ► oo (3.5) 

Ideally, the control objective could be achieved in minimum time by computing 
the PUMA joint displacements Ad d needed to cancel out the inertial-space error. 
However, exact cancellation would require complete knowledge of the disturbance 
signal. The next best solution then is to compute a Add which approximately cancels 
out the inertial-space error. With this goal in mind, the proposed control law is as 
follows: 


Add = 3 JUo k ) 3 0 R(y 0 )K c (°Ud - 0 u k ) (.3.6) 

where K c € 3? 6x6 is a matrix of control gains. Note that K c can be used to weight 
certain components of the inertial-space error less than others; for example, set- 
ting the first column of I\ c to zero would eliminate any control in the inertial X 
direction. Note also that this control law is essentially an inertial-space “spring”, 
whose “stiffness” is determined by K c . (Damping is assumed to be provided by the 
dynamic controller). Equation (3.6) will be referred to as the J x control law in the 
sequel. 

3.2.3 Closed-Loop System 

A simple expression for the closed-loop system can be derived by assuming 
that there is a one period delay in the control actuation: 


— Add 


(3.7) 
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Equation (3.7) basically means that the joint-level servo control is assumed to be 
“perfect”; i.e., the arm achieves the desired setpoint 9 d within one sample step of 
the J* controller. Substituting (3.6) and (3.7) into (3.4) results in 

°Uk — = ^R(t] 0 + Sk) 3 J3,e{^k) ^3,£(^fc-l)oR-( 7 7o)^"c( U d ~ u k-l) + Alik (3.8) 

In order to simplify this expression, define the quantity 

Mk,k-\ = ° 3 R{lo + S k ) 3 J 3 ,M) 3 JlM-i)oR(Vc)K (3-9) 

Rewriting (3.8) in terms of Mk,k-i, it is easy to see that the closed-loop system is 
linear with time- varying coefficients: 7 , - 

°Uk = (I — Mk,k-i)°idk-i + Mk,k-i°Ud 4- Avk (3.10) 

A block diagram of the closed-loop system is shown in Figure 3.1. 



Figure 3.1: Block Diagram of Closed-Loop System 
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3.3 Stability Analysis 

3.3.1 Spectrum of Closed-Loop System 

The stability of the closed-loop system can be completely characterized by 
the spectrum of I — Mk,k-i- The necessary and sufficient condition for stability is 
that, for all k > 0, the eigenvalues A,- of I — Mk,k-\ must lie in the unit circle in the 
A-plane: _ :: 


|A(7 — A/jfc,jfc-i)| < 1 V*> 0 (3.11) 

Equivalently, the eigenvalues A, of Mk,k-\ must lie in a circle of unit radius centered 
at the point (1.0, 0.0) in the A-plane. This can be verified by defining \ = 1 — Aj 
and substituting into the characteristic polynomial: 


p(A.) = det(A,-7 - (7 - M k ,k -\ )) 

= — det((l — A;)7 — Mk,k-i) 

= — det(A,7 — Mk, k -i) (3.12) 

Hence, the A,’s are the eigenvalues of Mk,k-i- 

The stability condition will now be expressed in terms of the matrix Mk,k-i- 
Define a to be the maximum angle of rotation of the eigenvalues of Mk,k-i • 

a = suparg(A t ) (3.13) 

t.fc 

and let (xo,yo) be a point on the shifted unit circle in the A-plane such that 
arg(xo + iyo) = a (see Figure 3.2). If p is the distance from the origin to (x 0 ,yo). 
then the stability criterion can be restated as follows: 


d-(Mk.k-i) < P Vfc > 0 


(3.14) 
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where <r{Mk,k-\) denotes the maximum singular value of Mk,k- 1 - 



Figure 3.2: Region of Stability in the A-Plane 

It is straightforward to find a relationship between a and p. The point (x 0 , y 0 ) 
must simultaneously satisfy the following set of equations: 


xl + yl = p 1 (3.15) 

(i 0 -l) 2 + yo = 1 (3.16) 

Solving for x 0 and y 0 gives 

2 y 

i x o,yo) = - /> 2 ) (3.17) 

Hence, a and p are related by 


* -l /J/°\ 
q = tan ( — ) 

Xq 
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,\/4 — /3 2 . 
= tan *(- — ) 


= tan ' 1 (\/3“ 1 ) 


or, solving for p, 


(3.18) 


^ \/tan 2 a + 1 

The condition for stability can therefore be written as 


(3.19) 


a{Mk,k-\) < 


Vk > 0 


(3.20) 


\/tan 2 q + 1 

This equation will be used in Section 3.3.3 to find an upper bound on the control 
gain, K c . As a first step toward deriving this bound, it is necessary to examine the 
spectrum of the matrix 6 J 3 , 9 6 J ^ , 9 . 

3.3.2 Spectrum of J J* 

Using the compact notation for 6 Jz, 9 , the matrix 6 J 3 , 9 6 J| 9 can be written as 


6 7 6 7 * — 

^3,9 ^3,9 — 


BO B' 0 

D E\{ -E'DB' E f 

BB f 0 

(I-EE')DB' EE 1 

Since this matrix is block triangular, the spectrum is simply the union of the eigen- 
values of BB * and EE U 


(3.21) 


A(V 3 ,9 6 4 9 ) = HBB') (J X(EP) (3.22) 

The following theorem completely specifies the spectrum of an arbitrary matrix 


times its pseudoinverse. 
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Theorem 3.1 If A € 9? mxn , with m<n, and rank(A) = r, then the spectrum of 
AA * consists of m — r eigenvalues at zero and r eigenvalues at one. 

i r-H " :7 .IT 

Proof: Let A = UHV 7 be the singular value decomposition of A. Then, 


AA* 


U'ZV T VZ'U T 
E r 0 
0 0 

Jr 0 

0 0 


u 


u 



ol 


1 

o 

o 


u 2 


u 1 


(3.23) 


Partition U into the column vectors [ui U 2 ■ ■ . «„]• Equation (3.23) can then be 
written els the sum of the outer products of the first r columns of U: 


AA* = Ujttf + U 2 «2 + ■ ■ • + U r u J 

= t «,)(«, (3.24) 

i=i 

The eigenvalues A, of AA* are the solutions to 


(AA% = A & (3.25) 

It will now be shown that the eigenvectors & are the columns of U and the corre- 
sponding eigenvalues are 



1 < i < r 


(0 r -f 1 < i < m 

First, consider 1 < i < r. Since U is orthonormal, then 


(3.26) 


for all j, k € {1 . . . r}, 


T 

Uj u k 


1 j ~ k 
.0 j / k 


(3.27) 
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Substituting <£,- = u,- in (3.25) and using Equations (3.24) and (3.27) results in 


{AA^)u t 


{uiuj + U 2 U 2 + • . . + U r ltr )Ui 


= Ui ufu,- + . . . + Uiujm + . ..U r ujui 

= Ui 

which implies that A, = 1. Now consider r + 1 < i < m. Since 


(3.28) 


uju k = 0 

for all j € {1 . . . r} and A: € {r + 1 . . . m}, then 


(3.29) 


(/L4 f )u,- = (UiuJ + U 2 U2 + . . . + U r llJ)Ui 

= 0 (3.30) 

which implies that A,- = 0. ■ 

Returning to the original problem, suppose that rank(B) = r and rank(£) = s. 
By Theorem 3.1, the complete spectrum of 6 J 3 , 9 6 Jz^ is 

A( 6 J 3 , 9 6 4 9 ) = {1_J,0,...,0} (3.31) 

3.3.3 Bound on Control Gain 

One final condition is needed to find an upper bound on A' c . Observe that, 
since J is a continuous operator. 


||WW(**-i)||-0 as ||A0fcjj — » 0 


(3.32) 
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In other words, for A 9k sufficiently small, J is approximately constant or slowly 
time-varying. Thus, for sufficiently small joint displacements, Mk,k- 1 — ► Mk, where 
Mk is defined as 

Mk = ° 3 R(Vo + Skfj 3 , E (ek) 3 jUek) 3 0 R(vo)K e (3.33) 

The results from Sections 3.3.1 and 3.3.2 can now be used to find a condition 
on K c for stability. Applying velocity and coordinate transformations to (3.33), 


Mk = ° 6 R(Vo + Sk,9k) 6 KM 6 J 3 A^) 6 JUek) 6 ^(^ 6 Q R(r] o ,0k)Kc (3.34) 


Since velocity and coordinate frame transformations are orthogonal, 


HMk) < H 6 A*(9k) 6 JU0k))cr(Kc) 
< &{K C ) 


Hence, a sufficient condition for stability is 


(3.35) 


a(K c ) < 


2 

V tan 2 a + 1 


3.4 Controller Design 
3.4.1 Attitude Error 


(3.36) 


An important design consideration is the method used to calculate the attitude 
error. So far, it has been assumed that the position and orientation of the end- 
effector are represented by the vectors °pk and °Oi. and the inertial-space error is 
computed as 



o o 

Ui — Uk = 


(3.37) 
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If the orientation is represented by the rotation matrix °R, however, then the com- 
ponents of °4>k must be extracted from before Equation (3.37) can be applied. 
Unfortunately, this approach runs into singularity problems at certain orientations. 
A more stable method is to use the attitude error matrix , defined as 

A R=° E R d ° E R T k (3.38) 

where ° R d and %R k are the desired and actual rotation matrices. In the limit as the 
rotations about the inertial X, Y, and Z axes approach zero, it can be shown [19] 
that 


AR -* dR 

1 — d<j> z d<Py 

d<j> z 1 -d<f> x (3.39) 

— d<$> y d(f) X 1 

The components d<p x , d<p y , and d<f> z represent the differential rotations about the 
inertial X , Y, and Z axes. Thus, for small (i.e., less than 180°) rotations about X, 
Y , and Z, the angular part of the inertial-space error can be formed by taking the 
(3,2), (1,3), and (2,1) components of AR: 


O' o 
9d — 9k 


Af?(3,2) Af?(l,3) AR(2, 1) 


1 T 


(3.40) 


3.4.2 Design Parameters 

The J : controller has two design parameters: the control gain, A' c , and the 
minimum singular value, <r mm . Some guidelines for selecting these parameters are 
discussed below. 

The selection of the control gain is greatly simplified by restricting I\ c to be a 
scalar times the identity matrix: 
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K e = k c I, 0 < k c < 2 (3.41) 

The parameter k c controls the spectral radius of Mk- For example, if k e = 0.5, then 
the eigenvalues of Mk will lie on a circle of radius 0.5 in the A-plane (or at zero, if J 
is singular). The region of stability can then be found by applying Equation (3.18). 
It is easy to verify that for k c = 0, the system can tolerate up to 90° rotation in 
the eigenvalues of Mk (i.e., a = 90°), and for k c = 2, the system is marginally stable 
(i.e., a = 0°). Thus, the choice of the control gain is a trade-off between performance 
(large k c ) and robustness (large a). 

It is straightforward to choose a stable k c if 6 is known a priori. (Recall that 6 
is the vector of maximum deviations in the platform joint positions.) Let A denote 
the spectrum of the matrix °R(r]o + S)lR(i] 0 ). By invoking the slowly time- varying 
condition, a can be approximated as follows: 


a « suparg(A) 


(3.42) 


and k c is calculated as 


k r = 


(3.43) 


\J tan 2 a + 1 

The selection of <7 mtn is essentially a trade-off between tracking accuracy and 
the norm of the control signal. Recall from Section 2.4.1 that increasing <r mm in- 
creases the width of the singular region and consequently reduces the norm of J* at 
the boundary of the singular region. In terms of disturbance rejection, increasing 
a m i n causes the control in the direction of the singularity to shut off earlier, result- 
ing in a larger tracking error. The advantage to increasing <r min is that the norm of 
Add will be smaller (and less discontinuous) at the boundary of the singular region. 
Therefore, the selection of <7 min should be based on the desired upper bound on the 
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norm of A 9d, which in turn is dictated by the saturation limits of the joint-level 
controller. 

3.5 Summary 

The design and analysis of a kinematic control law for inertial-space distur- 
bance rejection was described in this chapter. A discrete-time model of the closed- 
loop system was derived, and a sufficient condition for closed-loop stability was 
found. The selection of the controller design parameters and the computation of the 
attitude error were also discussed. 


CHAPTER 4 

IMPLEMENTATION ON A ROBOTIC TESTBED 


This chapter gives an overview of CIRSSE’s robotic testbed and some of the software 
used in the implementation of the 7* controller on the testbed. Section 4.1 describes 
the platform carts and the PUMA arms. Section 4.2 details the hardware-level 
interface and real-time operating systems. Sections 4.3 and 4.4 discuss the software 
used to control the robots, and Section 4.5 summarizes this chapter. 

4.1 Robot Hardware 

4.1.1 Platform Carts 

The platform system, custom built by K.N. Aronson, Inc. of Arcade, NY, 
consists of two 3-DOF carts on a 12 ft linear track. The platform joints are labeled 
1-3 for the left cart and 10 - 12 for the right cart. Joint 1 provides translational 
motion for the left cart along the track, while joints 2 and 3 provide tilt and pan, 
respectively. A diagram of the platform system is shown in Figure 4.1. 
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4.1.2 PUMA Arms 

Mounted on the platform system is a pair of 6-DOF PUMA arms, built by 
Unimation, Inc. of Danbury, CT. The joints of the left arm (Unimation model 560) 
are labeled 4-9 and the joints of the right arm (Unimation model 600) axe labeled 
13 - 18. The left PUMA and platform cart are shown in Figure 4.2. 



Figure 4.2: Left PUMA and Platform Cart 

Each PUMA arm is equipped with a force/torque sensor and a pneumatic 
gripper. Additionally, two cameras are mounted on a bracket located at the flange of 
the left PUMA. The physical dimensions of the force/torque sensor, camera mount, 
and gripper are taken into account by the tool transform, f.T, which specifies the 
position and orientation of the end-effector frame. For the left robot, the origin 

O 
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of frame E is located between the jaws of the gripper, 23.9 cm from the origin of 
frame 9 [20]. 

4.2 Computer Control System 

4.2.1 Hardware Interface 

The platform and PUMA robots are controlled from a VME chassis which 
contains a number of hardware components distributed across the bus. The bulk of 
the real-time computation takes place on three Motorola MVME 147SA-2 and two 
Motorola MVME 135 Single Board Computers (SBCs), labeled CPU 0 - CPU 5. 
A Motorola MVME 224-1 Shared Memory board provides a common address space 
for the CPUs. 

The platform encoders are accessed via three Whedco VME-3570-1 Dual Chan- 
nel Encoder Interface boards. A Datel DVME-628V D/A board is used to drive the 
platform servo motors. Digital lines, such as platform power, limit switches, and 
emergency stop switches are interfaced through a VME Microsystems VMIVME- 
2532A High-Voltage Digital I/O board. 

The encoder, torque, and power signals for the PUMA robots are handled by 
two Unimation controller boxes outside of the VME chassis. They are connected to 
the VME chassis by two VMEbus to Q-Bus adapters. 

The five SBCs are installed on an Ethernet backplane, which allows communi- 
cation between the VME chassis, a separate Datacube VME chassis (for computer 
vision), and the Sun workstations on the CIRSSE network. 

4.2.2 Operating Systems 

Each CPU runs under VxWorks 1 . a UNIX-compatible real-time operating sys- 
tem. Among other things, the VxWorks kerne! supports priority based scheduling, 
1 Wind River Systems, Alameda. CA. 
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intertask communication, synchronization, interrupt handling, and memory man- 
agement. 

However, VxWorks does not provide a mechanism for tasks on separate CPUs 
to communicate with each other. In order to facilitate interprocessor communica- 
tion, the CIRSSE Testbed Operating System (CTOS) was developed [21]. CTOS 
enables tasks to communicate asynchronously via message passing. 

In addition to interprocessor communication, CTOS also supports interchassis 
communication. For example, CTOS allows a task on CPU 5 to send and receive 
messages from a task on a Sun workstation (running under UNIX). This commu- 
nication bridges the gap between synchronous (real-time) and asynchronous (non- 
real-time) tasks. 

4.3 Motion Control System 

The Motion Control System (MCS) is a collection of real-time software com- 
ponents that provides joint-level servo control, force/torque- based control, setpoint 
interpolation, and trajectory generation. The portions of the MCS relevant to this 
report, as well as the software implementation of the J * control law, are discussed 
below. 

The MCS is loaded onto CPUs 0 - 5 at boot-time and can easily be configured 
to meet the needs of a particular experiment. For this thesis, the J * controller was 
used in place of the standard MCS trajectory generator. 

4.3.1 Channel I/O Drivers 

The platform and PUMA channel drivers are responsible for handling the robot 
I/O, including: torque commands, power and brake commands, emergency stop and 
limit switch status, encoder positions, and encoder calibration. The channel drivers 
run at the servo rate, which is typically 1/0.0045 s -1 . 
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The torque and position information for each joint is mapped onto a unique 
slot in shared memory, and can be accessed using the library chanLib. This allows 
tasks on other CPUs (e.g., servo controllers) to exchange data with the channel 
drivers in a synchronous fashion. Asynchronous information, such as power and 
calibration commands, is sent via CTOS messages. 

4.3.2 Inertial End-Point Sensor Driver 

A separate driver was written for this thesis to measure the location of the end- 
effector in inertial space. In lieu of a direct end-point sensor, the forward kinematics 
are used to compute °T k , the homogeneous transform describing the current position 
and attitude of the end-effector with respect to frame 0. (Note that this software is a 
temporary substitute for direct end-point feedback; the forward kinematics can not 
be used in practice since the platform joint positions are needed to calculate °7\ .1 
The end-effector transform is stored in a shared memory slot and can be accessed 
via the library chanlESLib. The inertial end-point sensor driver was implemented 
with a sampling rate of 1/0.0036 s* 1 . 

4.3.3 Joint-level Servo Controllers 

The platform and PUMA controllers compute the torques required to servo 
each joint to the desired setpoint. Position and velocity setpoints are passed to 
the controllers via the library interpLib, which uses linear interpolation to smooth 
the desired trajectory. The controllers run at the servo rate, in lock step with the 
channel drivers. 

The control algorithm for the PUMA is based on the well-known Proportional- 
plus-Integral-plus-Deri\ati\e (PID) control law [22. 23j. To reduce the coupling 
between the joints, the PID torques are multiplied by the diagonal terms of .\f[9). 


the mass matrix 2 . Gravity compensation was also added to further reduce the 
position error. Thus, the control law for the PUMA arm is 

r = M{& k ){Kp{e i - e k ) + k { - 8 k )AT + K D {d d - e k )) + g (e k ) (4.1) 

1=0 

where 

F is the 6x1 vector of joint torques 

M(9 k ) is the 6x6 mass matrix (diagonal terms only) 

8 d — 8 k is the 6x1 vector of position errors 
8 d — 8 k is the 6x1 vector of velocity errors 
Kp is a 6 x 6 diagonal matrix of proportional gains 

Ki is a 6 x 6 diagonal matrix of integral gains 

Kq is a 6 x 6 diagonal matrix of velocity gains 

g(9 k ) is the 6x1 vector of gravity torques 

AT is the sampling period 

In addition, a first order low-pass filter is used to attenuate the noise in the joint 
velocity estimates, 8 k . The control algorithm for the platform is identical to (4.1), 
with M = I and g — 0. 

4.3.4 J* Controller 

The J x controller functions like a trajectory generator, in that it supplies 
position setpoints to the PUMA servo controller through the interpLib inter- 
face (velocity setpoints are set to zero). The position setpoints are calculated by 
adding the control vector A 9 d to the the current joint positions 9 k , where A#; is 
computed as in Equation (3.6). The library jacLib. in particular the function 
jacPumaAprxPseudoInv(). is used to find the approximate pseudoinverse solution. 
The sampling rate of this controller is 1/0.00S1 s“ l . 

2 A 1(6) is the matrix that multiplies 9 in the Lagrange-Euler dynamics of the PUMA. 
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The inputs to the J * controller are the desired and current end-effector trans- 
forms, °T d and °T k , from which the inertial-space error is extracted (cf. Sec- 
tion 3.4.1). The current end-effector transform is read from shared memory using the 
inertial end-point sensor library described in Section 4.3.2. The desired transform is 
read from a file during controller initialization. Ideally, the desired transform would 
be specified on-line by a task-space trajectory generator; however, this functionality 
was not available at the time of this thesis. 

4.3.5 State Manager 

The MCS State Manager coordinates the bootstrapping phase of the Motion 
Control System by sending messages to the various components (channel drivers, 
controllers, etc.) at boot-time. The State Manager also implements a simple state 
machine for the testbed. The five states of the MCS are: 

Cold - MCS initialization. 

Reserve - Application reserves slots. 

Active - Robot power on, brakes on. 

Motion - Robot power on, brakes off. 

Emergency Stop - Emergency stop button pressed. Robot power off. 

4.4 Software Libraries 

In addition to the Motion Control System, several libraries of routines were 
used for this thesis. These libraries are briefly described below. 

4.4.1 Transform Library 

The Transform Library, or transLib, is a collection of routines that oper- 
ate on homogeneous transforms. In particular, the routines trans Invert () and 
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transMultiplyO are used by the J * controller to perform the transform inversion 
and multiplication required to compute Af?, the attitude error matrix. A CIRSSE 
Technical Memorandum describing the Transform Library is forthcoming. 

4.4.2 Kinematics Library 

The Kinematics Library, or kinLib, includes functions to perform the forward 
and inverse kinematics. The routine kinFwdO is used by the inertial end-point 
sensor driver to compute the current end-effector transform The functional 

interface for the Kinematics Library is described in [12]. 

4.4.3 Jacobian Library 

The Jacobian Library, or jacLib, contains routines for computing the solutions 
to the forward, forward transpose, inverse, pseudoinverse, and approximate pseu- 
doinverse Jacobian equations. The implementation details as well as the functional 
interface for the Jacobian Library are explained in [24]. 

The approximate pseudoinverse solution uses an algorithm developed by Press, 
Flannery, et al. [17] to perform the singular value decomposition. This algorithm 
can be found in the library slvLinEqn. 

4.5 Summary 

The major hardware and software components of the CIRSSE testbed were 
described in this chapter. The real-time implementation of the joint-level PID con- 
troller and the J* kinematic controller were discussed, as well as some of the sup- 
porting software, such as the inertial end-point sensor library and Jacobian library. 


CHAPTER 5 

EXPERIMENTAL RESULTS 


This chapter presents the results of four sets of experiments utilizing the testbed. 
The goal of the experiments was to demonstrate the performance of the J* con- 
trol law under various operating conditions. The first three sets of experiments 
focused on the time response of the closed-loop system for the following classes of 
disturbances: 

1. Step disturbances in the platform joints 

2. Sinusoidal disturbances in the platform joints 

3. Random disturbances in the platform joints 

The majority of disturbances that are likely to be encountered by the robot can be 
decomposed into signals belonging to these three classes; for example, an impulsive 
disturbance can be approximated as a combination of positive and negative step 
disturbances. The fourth set of experiments was aimed at understanding the open- 
loop characteristics of the control law in the neighborhood of singularities. 

This chapter is organized into five sections. Sections 5.1 - 5.3 discuss the 
performance of the J* control law for step, sinusoidal, and random disturbances 
in the platform rotation. (Results for the platform translational and tilt axes are 
qualitatively similar, and are excluded for the sake of brevity.) Section 5.4 examines 
the behavior of the J : control law near the singularities of the PUMA. Section 5.5 
presents some conclusions based on the experimental results. 

In this chapter, the term orientation error will refer to the equivalent angle of 
rotation Q. of the attitude error matrix AR (cf. Equation (3.38)). The orientation 
error is found by computing the equivalent axis and angle representation of AR: 
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A R = e U ', 0 < 4> e < 180° (5.1) 

where & € 3R 3 is the normalized axis of rotation and <j> e is the scalar representing the 
equivalent angle of rotation. An algorithm for extracting k and <p e from an attitude 
matrix is given in [19]. 


5.1 Step Disturbances in Platform Rotation 

This section analyzes the time response of the closed-loop system for 10°, 20°, 
and 30° step disturbances in the platform rotation. For each case, the control gain 
K c was set to identity. 

5.1.1 10° Step Disturbance 

Figure 5.1 shows the inertial-space errors errors when a 10° step disturbance 
is applied to the platform rotational joint. The linear ( X , Y, and Z ) components 
of the error are shown in the upper plot and the orientation error in the lower plot. 
The components of A 0d, the control vector, are plotted in Figure 5.2. 



Maximum Overshoot 

4% Settling Time 

1.527 xl0 +o cm 

1.54 s 

3.325 x 10 +o cm 

0.84 s 

6.366 x 10 -1 cm 

1.70 s 


1.00 s 



Table 5.1: Maximum Overshoot and 4% Settling Time for 10° Step 
Disturbance in Platform Rotation 


Table 5.1 lists the maximum overshoot and 49c settling time for the X, V, Z. 
and orientation errors. The 49c settling time refers to the time required for the error 
to enter and remain within ±c of zero, where e is 4% of the peak absolute error. 










Time (sec) 


Time (sec) 


Figure 5.1: Position Error ( X - solid curve; Y — dashed curve; Z - dot- 
ted curve) and Orientation Error for 10° Step Disturbance 
in Platform Rotation 


§ -5 


lime (sec) 


Figure 5.2: Control Signals (A0 d (l), A9 d (4) - solid curves; A^.(2 , A0*(5) 
- dashed curves; A^(3), A(9j(6) - dotted curves) for 10 a Step 
Disturbance in Platform Rotation 
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5.1.2 20° Step Disturbance 

Figures 5.3 and 5.4 show the inertial-space errors and control signals for a 20° 
step disturbance. The settling time and overshoot for X, Y, Z , and <p e are listed in 
Table 5.2. 



Time (see) 



Figure 5.3: Position Error (X — solid curve; Y — dashed curve; Z — dot- 
ted curve) and Orientation Error for 20° Step Disturbance 
in Platform Rotation 



Maximum Overshoot 

4% Settling Time 


5.526 x 10 +o cm 

1.60 s 

Y 

8.283 x 10 +o cm 

1.33 s 

Z 

2.435 x 10 +o cm 

2.06 s 

o e 

6. 806 x 10 tO deg | 

1.27 s 


Table 5.2: Maximum Overshoot and 4% Settling Time for 20 5 Step 
Disturbance in Platform Rotation 





Figure 5.4: Control Signals (A^(l), A^(4) — solid curves; A^(2), A^(5) 
- dashed curves; A^(3), A^(6) — dotted curves) for 20° Step 
Disturbance in Platform Rotation 


5.1.3 30° Step Disturbance 

The inertial-space errors and control signals for the 30° case are shown in 
Figures 5.5 and 5.6. The maximum overshoot and settling time for each coordinate 
are displayed in Table 5.3. 



Maximum Overshoot 

4% Settling Time 

X 

1.737 x 10 4 " 1 cm 

1.97 s 

Y 

1.706 xl0 +l cm 

2.43 s 

Z 

1.253 xl0 +1 cm 

1.66 s 


2.055 x 10 +1 deg 

2.08 s 


Table 5.3: Maximum Overshoot and 4% Settling Time for 30° Step 
Disturbance in Platform Rotation 


5.1.4 Comparison 

The maximum overshoot, and settling time provide a measure of the relative 
degree of stability of the closed-loop system. For example, the maximum overshoot 
in the X direction is about 1.5 cm for the 10 3 case. 3.5 cm for the 20° case, and 

17.4 cm for the 30° case, indicating that the degree of stability decreases as the 
magnitude of the disturbance increases. Likewise, the settling times are generally 
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Time (sec) 


Time (sec) 


Figure 5.5: Position Error (X — solid curve; Y — dashed curve; Z — dot- 
ted curve) and Orientation Error for 30 a Step Disturbance 
in Platform Rotation 


» 20f 

T3 


Time (sec) 


Figure 5.6: Control Signals (A0s(l), A^A-*) - solid curves; A<?i(2), A0*(5) 
- dashed curves; A0*(3), A#i(6) - dotted curves) for 30 a Step 
Disturbance in Platform Rotation 







longer for larger magnitude disturbances. 

Figure 5.7 shows a plot of the position error in the X - Y plane for 10°, 
20°, and 30° step disturbances. This view corresponds to looking in the negative Z 
direction, or “down”, from directly above the robot (see Figure 2.1). It is interesting 
to note that for the 10° case, the end-effector converges to the desired position along 
a roughly straight-line path, while for the 30° case, the path resembles a spiral. This 
spiraling is caused by the error in the approximate pseudoinverse Jacobian matrix, 
which is computed using the nominal platform position instead of the true platform 
position. 



Figure 5.7: Position Errors for 10° (solid curve), 20° (dashed curve), 
and 30° (dotted curve) Step Disturbances in Platform Ro- 
tation 
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5.2 Sinusoidal Disturbances in Platform Rotation 

This section compares the time response of the system (with K c = I) for 16 sec- 
ond, 8 second, and 4 second period sinusoidal disturbances in the platform rotation. 

5.2.1 16 Second Period Sinusoidal Disturbance 

Figure 5.8 shows the X, Y, and Z position errors, with and without distur- 
bance rejection, for a 10° amplitude, 16 second period sinusoidal disturbance in the 
platform rotation. The orientation error, with and without disturbance rejection, is 
shown in Figure 5.9. Table 5.4 displays the largest absolute error and mean-square 
error for each coordinate. 





Time (see) 

Figure 5.8: Position Errors (A r - solid curves; Y - dashed curves; Z - 
dotted curves) for 10° Amplitude, 16 Second Period Sinu- 
soidal Disturbance in Platform Rotation 
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Time (sec) 


Figure 5.9: Orientation Errors for 10° Amplitude, 16 Second Period 
Sinusoidal Disturbance in Platform Rotation 



Without Disturbance Rejection 

With Disturbance Rejection 

Max Error 

MSE 

Max Error 

MSE 

X 

4.414 xl0 +o cm 

9.641 xl0 +o cm 2 

3.240 xlO -1 cm 

7.220 xlO" 3 cm 2 

Y 

1.200 xl0 +1 cm 

6.915 x 10 +1 cm 2 

1.451 xlO +0 cm 

1.275 x 10 _l cm 2 

Z 

5.110 x 10 _1 cm 

2.511 xlO" 1 cm 2 

6.900 x 10 -2 cm | 

! 4.364 x 10” 4 cm 2 

4>e 

1.034 xl0 +1 deg 

5.332 xl0 +1 deg 2 

1.057 x 10 +o deg | 

7.178 xl0“ 2 deg 2 ! 


Table 5.4: Maximum and Mean-Square Errors for 10° Amplitude, 16 
Second Period Sinusoidal Disturbance in Platform Rota- 
tion 




5.2.2 8 Second Period Sinusoidal Disturbance 

Figures 5.10 and 5.11 show the position and orientation errors for a 10° am- 
plitude, 8 second period sinusoidal disturbance in the platform rotation. Table 5.5 
shows the maximum and mean-square position and orientation errors. 



Without Disturbance Rejection 

With Disturbance Rejection 

Max Error 

MSE 

Max Error 

MSE 


3.924 xl0 +o cm 



BiiTTrirtinM 

Y 

1.298 xl0 +1 cm 

8.166 xl0 +1 cm 2 

2.171 xl0 +o cm 

3.336 xlO" 1 cm 2 

Z 



1.310 x 10 -1 cm 

IQ^QEEfiEESI 

<i>t 

qqqEQSsHESS 

4.S65 x 10 +1 deg 2 




Table 5.5: Maximum and Mean-Square Errors for 10° Amplitude, 8 
Second Period Sinusoidal Disturbance in Platform Rota- 
tion 


5.2.3 4 Second Period Sinusoidal Disturbance 

Figures 5.12 - 5.13 show the position and orientation errors for a 10° amplitude, 
4 second period sinusoidal disturbance in the platform rotational joint. Table 5.6 
shows the maximum and mean-square errors for each coordinate. 



Without Disturbance Rejection 

With Disturbance Rejection 

Max Error MSE 

Max Error MSE 


4.364 x 10 +o cm | 4.135 x 1(T 0 cm 2 


Y 


2.989 x 10 +o cm I 1.731 x 10' rO cm 2 

Z 

4.610 x 10 1 cm j 6.697 x 10~ 2 cm 2 

2.690 x 10 _1 cm j 1.274 x 10“ 2 cm 2 | 

6' 

1.02S x 10 +l deg ! 4.512 xlO"’" 1 deg 2 

2.4S6 x 10 +o deg ' 1.174 x 10*° deg 2 j 


Table 5.6: Maximum and Mean-Square Errors for 10 ° Amplitude, 4 
Second Period Sinusoidal Disturbance in Platform Rota- 
tion 
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Figure 5 . 10 : Position Errors ( X - solid curves; Y — dashed curves; Z - 
dotted curves) for 10° Amplitude, 8 Second Period Sinu 
soidal Disturbance in Platform Rotation 
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Figure 5.11: Orientation Errors for 10° Amplitude, 8 Second Period Si- 
nusoidal Disturbance in Platform Rotation 
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Figure 5.12: Position Errors ( X - solid curves; Y — dashed curves; Z 
dotted curves) for 10° Amplitude, 4 Second Period Sinu 
soidal Disturbance in Platform Rotation 
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Figure 5.13: Orientation Errors for 10° Amplitude, 4 Second Period Si- 
nusoidal Disturbance in Platform Rotation 
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5.2.4 Comparison 

Comparing Figures 5.8 - 5.13, it can be concluded that the quality of the dis- 
turbance rejection diminishes as the frequency of the disturbance signal increases. 
One measure of the quality of the disturbance rejection is the mean-square error 
attenuation , defined as the ratio of the mean-square errors with and without distur- 
bance rejection, expressed in decibels: 

^mse = 201og 10 (£^ s /£ M s) dB (5.2) 

where £^ ts and £ms are the mean-square errors with and without disturbance re- 
jection, respectively. Table 5.7 lists the Amse values for the 16, 8, and 4 second 
period sinusoidal disturbances. Note that for the 8 second case, the j4mse value in 
the Z direction is positive, indicating that the error was amplified instead of atten- 
uated. However, the actual mean-square error in this case is only 2.5xl0 -3 cm 2 (see 
Table 5.5). 



T = 

16 s 


T 

2= 

8 s 


T 

= 

4 s 


X 

—6.251 x 

10 +1 

dB 

-4.229 

X 

10 Tl 

dB 

-3.122 

X 

10 +1 

dB 

Y 

—5.469 x 

10 +1 

dB 

-4.778 

X 

10 +1 

dB 

-3.285 

X 

10 +1 

dB 

Z 

—5.520 x 

10 +1 

dB 

+1.721 

X 

10 Tl 

dB 

-1.441 

X 

10 Tl 

dB 


-5.742 x 

10 +1 

dB 

-4.700 

X 

10+ 1 

dB j 

| -3.169 

X 

10 +1 

dB 


Table 5.7: Attenuation of Mean-Square Errors for 16, 8, and 4 Second 
Period, 10° Amplitude Sinusoidal Disturbances in Platform 
Rotation 


5.3 Random Disturbances in Platform Rotation 

Two types of stochastic disturbance signais are considered in this section: ran- 
dom noise with a uniform distribution, and random noise with a normal distribution. 
The control gain was set to identity, as in the previous sections. 
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5.3.1 Random Disturbance With Uniform Distribution 

Figures 5.14 and 5.15 show the position and orientation errors for a random 
noise disturbance in the platform rotation, uniformly distributed in the interval 
(—0.5°, +0.5°). The notation Unif(— 0.5°, +0.5°) will be used to represent this dis- 
turbance signal. Table 5.8 lists the maximum and mean-square for each coordinate. 



Without Disturbance Rejection 

With Disturbance Rejection 

Max Error 

MSE 

Max Error 

MSE 

X 

1.184 xl0 +o cm 

EEEEgEBEa 

8.440 xlO" 1 cm 

1.062 x 10" 1 cm 2 

Y 

5.939 xl0 +o cm 


3.799 xl0 +o cm 

2.362 xl0 +o cm 2 

Z 

3.010 x 10 _1 cm 


flWtrtrBIhB 


<t>e 

4.037 xl0 +o deg 

2.966 xl0 +o deg' 

2.911 xl0 +o deg 

1.397 xl0 +o deg 2 


Table 5.8: Maximum and Mean-Square Errors for Unif(— 0.5°, +0.5°) 
Random Disturbance in Platform Rotation 


5.3.2 Random Disturbance With Normal Distribution 

Figures 5.16 - 5.17 display the position and orientation errors for a zero mean, 
0.25° standard deviation Gaussian noise disturbance in the platform rotation (de- 
noted by jV(0, 0.25°)). The maximum and mean-square errors are given in Table 5.9. 



Without Disturbance Rejection 

With Disturbance Rejection 

Max Error 

MSE 

Max Error 

MSE 

E9 


6.275 x 10 -1 cm 2 

8.760 x 10 _1 cm 

1.373 xlO* 1 cm 2 

Y 

5.501 x 10 +o cm 

6.625 x 10 +o cm 2 

3.739 xl0 +o cm 

2.534 x 10 +o cm 2 

Z 


1.304 x 10~ 3 cm 2 



6' 

KTirrmwmB !m fiTViirTTj 

2.899 x 10 +o deg 

1.515 xl0 +o deg 2 


Table 5.9: Maximum and Mean-Square Errors for A”(0.0.25 3 ) Random 
Disturbance in Platform Rotation 
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Figure 5.14: Position Errors (.Y — solid curves; Y — dashed curves; Z - 
dotted curves) for Unif(-0.5°, +0.5°) Random Disturbance 
in Platform Rotation 
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Figure 5.15: Orientation Errors for Unif(-0.o°, +0.5° j Random Distur- 
bance in Platform Rotation 
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Figure 5.16: Position Errors (X - solid curves; Y — dashed curves; Z - 
dotted curves) for ;V(0, 0.25°) Random Disturbance in Plat 
form Rotation 
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Figure 5.17: Orientation Errors for .V'(0.0.25°) Random Disturbance in 
Platform Rotation 
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5.3.3 Comparison 

Table 5.10 displays the mean-square error attenuations for uniform and Gaus- 
sian random noise in the platform rotation. The Amse values indicate that the 
performance is similar for both cases. In comparison with the results for sinusoidal 
disturbances, however, the quality of disturbance rejection is significantly less, since 
the random disturbance signals are of much higher bandwidth. 



Unif(— 0.5°, +0.5°) 

N (0,0.25°) 

X 

-7.022 xl0 +o dB 

-1.320 xl0 +1 dB 

Y 

-8.807 xl0 +o dB 

-8.347 xl0 +o dB 

Z 

-3.196 xl0 +1 dB 

+1.272 xl0 +1 dB 

<f>< 

-6.542 x 10 +o dB 

-1.093 xl0 +1 dB 


Table 5.10: Attenuation of Mean-Square Errors for Unif(— 0.5°, +0.5°) 
and A/*(0,0.25°) Random Disturbances in Platform Rotation 


5.4 Behavior Near Singularities 

In the experiments discussed so far, the manipulator was able to maintain the 
desired end-effector position and orientation without being forced into a singular 
configuration. This section examines the behavior of the J x controller when the arm 
is at or near each of the three PUMA singularities. 

5.4.1 Arm Fully Stretched Singularity 

Figure 5.18 shows the vector of open-loop control signals near the Arm Fully 
Stretched singularity. The minimum singular value parameter. <r mtn , was set to 
0.1. At this value of a m!n . the control in the direction of the workspace bounaarv 
becomes very weak approximately 30° from the singular point. This prevents the 
end-effector from getting too close to the workspace boundary. Consequentlv. the 
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manipulator will not switch between the flex and nof lex configurations while the 
J* controller is running. 

If the parameter <7 m ,„ is sufficiently small, however, the width of the singular 
region will be reduced to the point where the control signal for joint 6 (A^(3)) 
could drive the arm through the singularity. This may lead to an undesirable “chat- 
tering” behavior, in which the arm rapidly oscillates between the flex and noflex 
configurations^ 
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Figure 5.18: Behavior of l/det(J) and Open-Loop Control Signals 
(A#i(l), A0j(4) - solid curves; A0j(2), A^(-5) - dashed 
curves; A^(3), A0<*(6) - dotted curves) Near Arm Fully 
Stretched Singularity 


5.4.2 Hand Over Head Singularity 

Figure 5.19 shows the open-loop control signals in the vicinity of the Hand 
Over Head singularity, with <r min — 0.1. At about 10° from the singular point, the 
control in the “forbidden -1 directions (c.f. Section 2.1.4) becomes very weak. Unlike 




62 


the Arm Fully Stretched singularity, this does not prevent the manipulator from 
changing configurations; however, it does mean that the end-effector will be unable 
to track certain linear components of the desired trajectory while the arm is in the 
singular region. 




Figure 5.19: Behavior of l/det(J) and Open-Loop Control Signals 
(A^(l), A^(4) - solid curves; A^(2), A0 d (o) - dashed 
curves; A^(3), A^(6) - dotted curves) Near Hand Over 
Head Singularity 


5.4.3 Wrist Singularity 

Figure 5.20 shows the open-loop control signals near the Wrist singularity, with 
<7min = 0.1. The control signals for joints 7 and 9 (A^'(4) and A#,j(6)) go to zero 
about 8° from the singular point. As in the Hand Over Head singularity, this does 
affect the ability to change configurations. However, the end-effector will be unable 
to track certain angular components of the desired trajectory when the position of 
joint 3 is within 8° of zero. 
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Figure 5.20: Behavior of l/det(J) and Open-Loop Control Signals 
(A9j(l), A9 d (4) - solid curves; A9 d (2), A9 d ( 5) - dashed 
curves; A0j(3), A0 j(6) — dotted curves) Near Wrist Singu- 
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5.5 Summary 

Several important conclusions can be drawn from the experimental results 
presented in this chapter. These conclusions are summarized below: 

1. The relative stability of the closed-loop system is a function of the amplitude 
of the disturbance signal. 

By comparing the maximum overshoot and settling time, it was argued that the 
system was less stable for the 30° step input than for the 10° step input. This 
observation agrees with the stability analysis presented in Section 3.3, since a is 
directly related to the maximum disturbance amplitude (cf. Equation 3.42). With 
K c = I, the system is stable for platform rotational disturbances less than 60°. 

2. The relative performance of the controller is a function of the frequency of the 
disturbance signal. 

For the 16 second sinusoidal disturbance, the mean-square error attenuation was very 
good (about -55 dB), but for the 4 second sinusoi d al disturbance, the attenuation 
was markedly less (about -30 dB). In other words, the J* controller is like a high-pass 
filter; the lowest frequency components of the disturbance signal are attenuated the 
most. 

3. The control in certain directions becomes very weak near singularities. 

This implies that there may be an unavoidable tracking error in the “forbidden 7 ’ 
directions when the arm is at or near a singularity. This also prevents the arm 
from switching between the flex and noflex configurations near the workspace 
boundary. 


CHAPTER 6 
CONCLUSION 


6.1 Report Summary and Conclusions 

This report described the design, analysis, implementation, and performance 
of a kinematic controller for inertial-space disturbance rejection. First, the problem 
of mapping end-effector displacements to joint displacements was considered. The 
approximate pseudoinverse Jacobian was presented as a computationally efficient 
and well-defined solution to this problem. Next, a kinematic control algorithm, 
the J * control law, was proposed. A discrete-time model of the closed-loop system 
was derived, and the stability of the system was shown to be related to the upper 
bound on the disturbance and the selection of the control gain. The real-time 
implementation of the controller on CIRSSE’s robotic testbed was then discussed, 
as well as the hardware and software components of the testbed used in this thesis. 
Finally, some experimental results were presented, comparing the performance of the 
controller for step, sinusoidal, and random disturbances in the platform rotational 
axis, and at the singularities of the PUMA. 

In conclusion, the J x controller has been demonstrated to be very effective 
for rejecting the low-frequency components of an arbitrary disturbance signal. The 
controller was shown to be robust with respect to relatively large magnitude distur- 
bances and in the neighborhood of kinematic singularities. The modest computa- 
tional requirements of the algorithm, coupled with the fact that precise knowledge 
of the disturbance signal is not required, suggest that this controller is a practical 
solution to the inertial-space disturbance rejection control problem. 
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6.2 Future Research 

Several recommendations for future directions in this area of research are dis- 
cussed below. 

1. Implement direct inertial end-point sensing. 

Recall from Section 4.3.2 that the inertial end-point sensor driver calculates the 
inertial end-effector position and attitude using the forward kinematics. In practice, 
however, additional sensors are needed to measure the end-effector location since 
the platform joints are not exactly known. This driver should be replaced when 
direct end-point feedback is available. 

2. Incorporate joint limit constraints into the kinematic control algorithm. 

Although the joint limits of the manipulator are usually taken into account by the 
path planner, a large enough magnitude disturbance could force one or more joints to 
its limit. It would be desirable to avoid this situation by augmenting the kinematic 
control law with joint limit constraints. 

3. Design a better dynamic control algorithm. 

The performance of the system could be improved by using a better joint-level 
controller. The limiting factor in the PID control algorithm used for this research 
appears to be velocity noise, which arises from backward differencing the joint po- 
sitions. This noise could be reduced by Kalman filtering or by directly measuring 
the joint velocities with tachometers. 

4. Investigate alternative kinematic control algorithms. 

Several alternatives to the approximate pseudoinverse Jacobian exist for transform- 
ing between joint and task space. For instance, the inverse kinematics could be used 
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to map the inertial-space position and attitude to joint positions, or the transpose of 
the Jacobian could be used to map a a force-like” error (based on the inertial-space 
error) to joint torques. 

5. Extend the results to free-floating space manipulator systems. 

There are three issues which arise when dealing with free-floating systems that were 
not specifically addressed in this report. First, the dynamics of free-floating systems 
are more complicated than those of terrestrial systems; for example, there may be 
significant dynamic coupling between the manipulator and spacecraft, causing the 
spacecraft to react to manipulator motions. Second, the Jacobian of a free-floating 
system depends not only on the joint angles and kinematic parameters, but on the 
system mass and inertia properties [3], Finally, a space manipulator may encounter 
dynamic singularities, depending on the history of the manipulator motion [3, 25]. 
The results presented in this report should be extended to encompass free-floating 
systems when these issues are better understood. 
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Figure 2.1: Coordinate Frame Assignments 
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Figure 2.2: 2-Norms of (solid curve), J 1 (dashed curve), and J~ l 
(dotted curve) Near Hand Over Head Singularity 
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Figure 2.3: 2-Norms of J t (solid curve), J* (dashed curve), and J~ l 
(dotted curve) Near Arm Fully Stretched Singularity 
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Figure 2.4: 2-Norms of J x (solid curve), J x (dashed curve), and J 
(dotted curve) Near Wrist Singularity 
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Figure 4.1: 3-DOF Platform Carts 
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Figure 5.1: Position Error (.Y - solid curve; Y - dashed curve; Z - dot- 
ted curve) and Orientation Error for 10° Step Disturbance , 

in Platform Rotation ’ 
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Figure 5.2: Control Signals (A^(l), A^(4) - solid curves; A^(2), A0 d (o) 
- dashed curves; A^(3), A9 d (6) - dotted curves) for 10° Step 
Disturbance in Platform Rotation 
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Figure 5.3: Position Error (X - solid curve; Y - dashed curve; Z - dot- 
ted curve) and Orientation Error for 20° Step Disturbance 
in Platform Rotation 
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Figure 5.5: Position Error (X - solid curve; Y - dashed curve; Z - dot- 
ted curve) and Orientation Error for 30° Step Disturbance 
in Platform Rotation 






Figure 5.6: Control Signals (A9 d (l), A<9*(4) - solid curves; A9 d (2), A0 d (o) 
- dashed curves; A0 d (3), A0 d (6) - dotted curves) for 30° Step 
Disturbance in Platform Rotation 
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Figure 5.7: Position Errors for 10° (solid curve), 20° (dashed curve), 
and 30° (dotted curve) Step Disturbances in Platform Ro- 
tation 
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Figure 5.8: Position Errors (.Y — solid curves; Y — dashed curves; Z — 
dotted curves) for 10° Amplitude, 16 Second Period Sinu- 
soidal Disturbance in Platform Rotation 
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Figure 5.9: Orientation Errors for 10° Amplitude, 16 Second Period 
Sinusoidal Disturbance in Platform Rotation 
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Figure 5.10: Position Errors („Y — solid curves; V — dashed curves; Z 
dotted curves) for 10° Amplitude, 8 Second Period Sin 
soidal Disturbance in Platform Rotation 
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Figure 5.11: Orientation Errors for 10° Amplitude, 8 Second Period Si- 
nusoidal Disturbance in Platform Rotation 
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Figure 5.12: Position Errors (X - solid curves; Y - dashed curves; Z 
dotted curves) for 10 s Amplitude, 4 Second Period Sin 
soidal Disturbance in Platform Rotation 
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Figure 5.13: Orientation Errors for 10° Amplitude, 4 Second Period Si- 
nusoidal Disturbance in Platform Rotation 
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Figure 5.14: Position Errors ( X — solid curves; Y — dashed curves; Z — 
dotted curves) for Unif(-0.5°, +0.5°) Random Disturbance 
in Platform Rotation 
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Figure 5.15: Orientation Errors for Unif(-0.5°, +0.5°) Random Distur- 
bance in Platform Rotation 
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Figure 5.17: Orientation Errors for jV(0, 0.25°) Random Disturbance in 
Platform Rotation 
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Figure 5.18: Behavior of l/det(J) and Open-Loop Control Signals 
(A6 d (l), A9 d (4) - solid curves; A0 d (2), A^(5) - dashed 
curves; A^(3), A^(6) - dotted curves) Near Arm Fully 
Stretched Singularity 
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Figure 5.19: Behavior of l/det(J) and Open-Loop Control Signals 
(A0 rf (l), A9 d (4) - solid curves; A9 d (2), A9 d (5) - dashed 
curves; A9 d (3), A^(6) - dotted curves) Near Hand Over 
Head Singularity 
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